/**************************************************************
WinFilter version 0.8
http://www.winfilter.20m.com
akundert@hotmail.com

Filter type: Low Pass
Filter model: Butterworth
Filter order: 18
Sampling Frequency: 10 Hz
Cut Frequency: 1.000000 Hz
Coefficents Quantization: float

Z domain Zeros
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000
z = -1.000000 + j 0.000000

Z domain Poles
z = 0.473958 + j -0.171172
z = 0.473958 + j 0.171172
z = 0.605404 + j -0.279558
z = 0.605404 + j 0.279558
z = 0.621711 + j -0.095059
z = 0.621711 + j 0.095059
z = 0.530534 + j -0.270847
z = 0.530534 + j 0.270847
z = 0.446040 + j -0.058073
z = 0.446040 + j 0.058073
z = 0.598209 + j -0.362644
z = 0.598209 + j 0.362644
z = 0.648892 + j -0.427535
z = 0.648892 + j 0.427535
z = 0.702212 + j -0.492677
z = 0.702212 + j 0.492677
z = 0.769587 + j -0.557015
z = 0.769587 + j 0.557015
***************************************************************/
#include "board_filter.h"
#define DIMENTION 4
#define Ntap 31




float fir(float NewSample) {
    float FIRCoef[Ntap] = {
        -0.00511986286346815480,
        0.00579136825588673790,
        0.01643120354048691100,
        0.02200852651984051500,
        0.01884938163298433300,
        0.00620199646243888450,
        -0.01276621294457447500,
        -0.03131707774659913500,
        -0.04092855261101093600,
        -0.03406719648892955100,
        -0.00705118276065129500,
        0.03804007748091180900,
        0.09321536604807120800,
        0.14641364806204205000,
        0.18486033566305940000,
        0.19887636349902357000,
        0.18486033566305940000,
        0.14641364806204205000,
        0.09321536604807120800,
        0.03804007748091180900,
        -0.00705118276065129500,
        -0.03406719648892955100,
        -0.04092855261101093600,
        -0.03131707774659913500,
        -0.01276621294457447500,
        0.00620199646243888450,
        0.01884938163298433300,
        0.02200852651984051500,
        0.01643120354048691100,
        0.00579136825588673790,
        -0.00511986286346815480
    };

    static float x[Ntap]; //input samples
    float y=0;            //output sample
    int n;

    //shift the old samples
    for(n=Ntap-1; n>0; n--)
       x[n] = x[n-1];

    //Calculate the new output
    x[0] = NewSample;
    for(n=0; n<Ntap; n++)
        y += FIRCoef[n] * x[n];

    return y;
}




float median_average(float input_data)
{
    static float median_array[DIMENTION];
    static float *data_pointer = median_array;
    static  float array_summ=0;

    array_summ = array_summ - *data_pointer + input_data;
    *data_pointer = input_data;
    data_pointer++;
    if(data_pointer > &median_array[DIMENTION-1] )
    {
        data_pointer = median_array;
    }
    return(array_summ/DIMENTION);
}

//Here should be filter implementation
float filterAccelX(float x)
{
  return x;
}
float filterAccelY(float y)
{
  return y;
}
float filterAccelZ(float z)
{
  return z;
}
float filterGyroX(float x)
{
  return x;
}

float filterGyroY(float y)
{
  return y;
}

float filterGyroZ(float z)
{
  return z;
}

float filterMagnX(float x)
{
  return x;
}

float filterMagnY(float y)
{
  return y;
}

float filterMagnZ(float z)
{
  return z;
}












